
#include <iostream>
#include <Eigen/Dense>
#include <math.h>

#include "KinematicModelTypes.h"
#include "Point.h"

dirKinResult dirKin(double teta1, double teta2, double teta3, double teta4);
invKinResult invKin(Point p);
//invKinResult invKin(double x, double y, double z, bool recursive);*/

int main(int argc, char* argv[]){
	
	if(argc == 4){
		Point p = Point(atof(argv[1]),atof(argv[2]),atof(argv[3]));
		invKinResult ikr = invKin(p);
		std::cout << "Risultato:\n" << ikr.teta1 << "\n" << ikr.teta2 << "\n" << ikr.teta3 << std::endl;
	}
	if(argc == 5){
		dirKinResult dkr = dirKin(atof(argv[1]), atof(argv[2]), atof(argv[3]), 0.0);
		std::cout << "Risultato:\n" << dkr.handCenter.getValue(Point::X) << "\n" << dkr.handCenter.getValue(Point::Y) << "\n" << dkr.handCenter.getValue(Point::Z) << std::endl;
	}
}